

void doControl(void) {
    
  // Get Gyro
  getGyro();
  
  // Get Accelerometer
  getAccel();
  
  // Get Compass Heading
  getCompass();
  
  // Process Altitude & Velocity
  agl = rangeFilter.update(10 * sRange, tUpdate);
  rangeRate = rangeFilter.vOut;
  
  // service pressure sensor
  if (t_run) {
    // if we started temp measurement, then get temp
    pAlt.get_temp();
    t_run = false;
  } else  {
    // else get the pressure measurement
    pAlt.pressure();
    // and determine if we need a temp or pressure
    if (++t_ctr >= do_temp) {
      t_ctr = 0;
      pAlt.start_temp();
      t_run = true;
    } else {
      pAlt.start_pressure();
      t_run = false;
    }
  }
  
  getGPSvel();
  
  inu.update(accel.r, gyroRate.r, gpsVelocity.r, true, compass_head, tUpdate);
  
  // get battery
  battery = analogRead(V_BATT);
  
  
  
}
